function [F_in, M_in, F_plant, M_plant, qi_d, qi, a_all, un_i, sOmega, sOmega_d, angleid_p, Ome_L] = controller_o_i(x_new, qi_d, sOmega, sOmega_d, qi, Att_L_d, Ome_L, Ome_L_d, pL_d, pL_d_dot, angleid_p,rho)
%% Parameters
global g dt m_o m_p
global a1_p a2_p a3_p
global b1_p b2_p b3_p
a_p = [a1_p, a2_p, a3_p];
b_p = [b1_p, b2_p, b3_p];

l1 = 1; l2 = l1; 
l3 = l1; l4 = l1;

%% columns of x input
format long;

%% Obtain adjacency matrix & reassign values to x

% 几何中心位置
x_L = x_new(2:end,5);
x_plant = x_new(2:end,1:4);

%% 物体位置和姿态
x_Pos_L = [x_L(7); x_L(9);  x_L(11)];       % X Y Z
x_Vel_L = [x_L(8); x_L(10); x_L(12)];       % X_dot Y_dot Z_dot
x_Att_L = [x_L(1); x_L(3);  x_L(5)];        % PHI（roll） THETA（pitch） PSI（yaw）
x_Ome_L = [x_L(2); x_L(4);  x_L(6)];        % PHI_dot THETA_dot PSI_dot
x_Ome_L_dot = (x_Ome_L - Ome_L) / dt;

%% 物体的角度控制率
% 按照 ZYX 顺序，把 (yaw, pitch, roll) 转换成旋转矩阵
RL = eul2rotm_ZYX(x_Att_L);
% % 计算姿态误差
e_Att_L = x_Att_L - Att_L_d;
e_Ome_L = x_Ome_L - Ome_L_d;
k_RL = 80;   
k_omega_L = 3.9;

Md = -k_RL * e_Att_L - k_omega_L * e_Ome_L;

%% 物体的位置控制率
% 位置误差
e_p_L = x_Pos_L - pL_d;
e_v_L = x_Vel_L - pL_d_dot;
e3 = [0; 0; 1];

% k_p_L = diag([40, 40, 80]);
% k_v_L = diag([1, 20, 80]);
k_p_L = diag([40, 40, 80]);
k_v_L = diag([1, 20, 80]);

Fd = -k_p_L * e_p_L - k_v_L * e_v_L - m_o * g * e3;


rho_hat = zeros(3, 3, 4);
for rho_num = 1 : 4
    rho_hat(:,:,rho_num) = skew(rho(:,rho_num));
end


I33 = eye(3);
P = [I33,           I33,           I33,           I33;
     rho_hat(:,:,1),rho_hat(:,:,2),rho_hat(:,:,3),rho_hat(:,:,4)];

PT    = P';
RL_diag = kron(eye(4), RL);
if rank(P*PT) < size(P*PT,1)
    warning('P*PT 可能奇异，使用伪逆');
    PPinv = pinv(P*PT);
else
    PPinv = inv(P*PT);
end
% PPinv = (P*PT) \ eye(size(P,1));

temp_vec = [RL'*Fd;
            Md];

miud = RL_diag * PT * PPinv * temp_vec;
miu1d = miud(1:3, :); 
miu2d = miud(4:6, :); 
miu3d = miud(7:9, :); 
miu4d = miud(10:12,:);


% 求解新qi，无人机质心指向连接点的位置的单位向量
x_pos = zeros(3,4);
x_pos(:,1) = [x_plant(7,1); x_plant(9,1); x_plant(11,1)];
x_pos(:,2) = [x_plant(7,2); x_plant(9,2); x_plant(11,2)];
x_pos(:,3) = [x_plant(7,3); x_plant(9,3); x_plant(11,3)];
x_pos(:,4) = [x_plant(7,4); x_plant(9,4); x_plant(11,4)];

% 求解当前q（无人机指向连接点的方向向量）
% 连接点的全局坐标
x_rho_global = zeros(3,4);
x_rho_global(:,1) = x_Pos_L + RL * rho(:,1);
x_rho_global(:,2) = x_Pos_L + RL * rho(:,2);
x_rho_global(:,3) = x_Pos_L + RL * rho(:,3);
x_rho_global(:,4) = x_Pos_L + RL * rho(:,4);

denom1 = norm(x_rho_global(:,1) - x_pos(:,1)); 
denom2 = norm(x_rho_global(:,2) - x_pos(:,2));
denom3 = norm(x_rho_global(:,3) - x_pos(:,3)); 
denom4 = norm(x_rho_global(:,4) - x_pos(:,4));

if denom1 < 1e-6
    denom1 = 1e-6;
end
if denom2 < 1e-6
    denom2 = 1e-6;
end
if denom3 < 1e-6
    denom3 = 1e-6;
end
if denom4 < 1e-6
    denom4 = 1e-6;
end
% q
q1 = (x_rho_global(:,1) - x_pos(:,1)) / denom1; 
q2 = (x_rho_global(:,2) - x_pos(:,2)) / denom2;
q3 = (x_rho_global(:,3) - x_pos(:,3)) / denom3;
q4 = (x_rho_global(:,4) - x_pos(:,4)) / denom4;
% q_dot
q1_dot = (q1 - qi(:,1)) / dt; 
q2_dot = (q2 - qi(:,2)) / dt;
q3_dot = (q3 - qi(:,3)) / dt; 
q4_dot = (q4 - qi(:,4)) / dt;

% q_hat
q1_hat = skew(q1);
q2_hat = skew(q2); 
q3_hat = skew(q3); 
q4_hat = skew(q4);

% 求miu来解实际输入量
miu1 = q1 * q1'*miu1d;
miu2 = q2 * q2'*miu2d; 
miu3 = q3 * q3'*miu3d; 
miu4 = q4 * q4'*miu4d; 

% 输入F_in
F_in = miu1 + miu2 + miu3 + miu4;

%% 计算全局坐标系下的力臂向量 M_in
r1_global = RL * rho(:,1);
r2_global = RL * rho(:,2);
r3_global = RL * rho(:,3);
r4_global = RL * rho(:,4);

%% 计算全局坐标系下的总力矩
M_inertial_1 = skew(r1_global) * miu1;
M_inertial_2 = skew(r2_global) * miu2;
M_inertial_3 = skew(r3_global) * miu3;
M_inertial_4 = skew(r4_global) * miu4;
M_inertial = M_inertial_1 + M_inertial_2 + M_inertial_3 + M_inertial_4;
%% 转换到体坐标系
M_in = RL' * M_inertial;

% M_in_1 = rho_hat(:,:,1) * RL' * miu1;
% M_in_2 = rho_hat(:,:,2) * RL' * miu2;
% M_in_3 = rho_hat(:,:,3) * RL' * miu3;
% M_in_4 = rho_hat(:,:,4) * RL' * miu4;
% M_in = M_in_1 + M_in_2 + M_in_3 + M_in_4;

q1d = - miu1d / norm(miu1d); 
q2d = - miu2d / norm(miu2d);
q3d = - miu3d / norm(miu3d);
q4d = - miu4d / norm(miu4d);

% q_i_d_0用来迭代
q1d_dot = (q1d - qi_d(:,1)) / dt; 
q2d_dot = (q2d - qi_d(:,2)) / dt;
q3d_dot = (q3d - qi_d(:,3)) / dt;
q4d_dot = (q4d - qi_d(:,4)) / dt;

% 计算绳索的角速度预期值sOmega_d
sOmega1d = cross(q1d, q1d_dot);
sOmega2d = cross(q2d, q2d_dot);
sOmega3d = cross(q3d, q3d_dot);
sOmega4d = cross(q4d, q4d_dot);

sOmega1d_dot = (sOmega1d - sOmega_d(:,1)) / dt;
sOmega2d_dot = (sOmega2d - sOmega_d(:,2)) / dt;
sOmega3d_dot = (sOmega3d - sOmega_d(:,3)) / dt;
sOmega4d_dot = (sOmega4d - sOmega_d(:,4)) / dt;

e_q1 = cross(q1d, q1);
e_q2 = cross(q2d, q2);
e_q3 = cross(q3d, q3);
e_q4 = cross(q4d, q4);

% 这里的sOmega需要将上一时刻的传入
e_sOmega1 = sOmega(:,1) + q1_hat * q1_hat * sOmega1d;
e_sOmega2 = sOmega(:,2) + q2_hat * q2_hat * sOmega2d;
e_sOmega3 = sOmega(:,3) + q3_hat * q3_hat * sOmega3d;
e_sOmega4 = sOmega(:,4) + q4_hat * q4_hat * sOmega4d;

% 货物omega
x_Ome_L_hat = skew(x_Ome_L);

% a_1 = (1/m_o) * F_in + RL  * x_Ome_L_hat * x_Ome_L_hat * rho(:,1) + RL * rho_hat(:,:,1) / J0 * (x_Ome_L_hat * J0 * x_Ome_L - M_in);
% a_2 = (1/m_o) * F_in + RL  * x_Ome_L_hat * x_Ome_L_hat * rho(:,2) + RL * rho_hat(:,:,2) / J0 * (x_Ome_L_hat * J0 * x_Ome_L - M_in);
% a_3 = (1/m_o) * F_in + RL  * x_Ome_L_hat * x_Ome_L_hat * rho(:,3) + RL * rho_hat(:,:,3) / J0 * (x_Ome_L_hat * J0 * x_Ome_L - M_in);
% a_4 = (1/m_o) * F_in + RL  * x_Ome_L_hat * x_Ome_L_hat * rho(:,4) + RL * rho_hat(:,:,4) / J0 * (x_Ome_L_hat * J0 * x_Ome_L - M_in);
% a_all = [a_1, a_2, a_3, a_4];

a_1 = (1/m_o) * F_in + RL * x_Ome_L_hat * x_Ome_L_hat * rho(:,1) - RL * rho_hat(:,:,1) * x_Ome_L_dot;
a_2 = (1/m_o) * F_in + RL * x_Ome_L_hat * x_Ome_L_hat * rho(:,2) - RL * rho_hat(:,:,2) * x_Ome_L_dot;
a_3 = (1/m_o) * F_in + RL * x_Ome_L_hat * x_Ome_L_hat * rho(:,3) - RL * rho_hat(:,:,3) * x_Ome_L_dot;
a_4 = (1/m_o) * F_in + RL * x_Ome_L_hat * x_Ome_L_hat * rho(:,4) - RL * rho_hat(:,:,4) * x_Ome_L_dot;
a_all = [a_1, a_2, a_3, a_4];

% 求解un
% sOmega 绳索
K_sOmega = 2;
K_q = 2;

un_1 = m_p(1) * l1 * q1_hat * (-K_q * e_q1 - K_sOmega * e_sOmega1 - (q1' * sOmega1d) * q1_dot + sOmega1d_dot) - m_p(1) * q1_hat^2 * a_1;
un_2 = m_p(2) * l2 * q2_hat * (-K_q * e_q2 - K_sOmega * e_sOmega2 - (q2' * sOmega2d) * q2_dot + sOmega2d_dot) - m_p(2) * q2_hat^2 * a_2;
un_3 = m_p(3) * l3 * q3_hat * (-K_q * e_q3 - K_sOmega * e_sOmega3 - (q3' * sOmega3d) * q3_dot + sOmega3d_dot) - m_p(3) * q3_hat^2 * a_3;
un_4 = m_p(4) * l4 * q4_hat * (-K_q * e_q4 - K_sOmega * e_sOmega4 - (q4' * sOmega4d) * q4_dot + sOmega4d_dot) - m_p(4) * q4_hat^2 * a_4;
% un_1 = m_p(1) * l1 * q1_hat * (-K_q * e_q1 - K_sOmega * e_sOmega1) - m_p(1) * q1_hat^2 * a_1;
% un_2 = m_p(2) * l2 * q2_hat * (-K_q * e_q2 - K_sOmega * e_sOmega2) - m_p(2) * q2_hat^2 * a_2;
% un_3 = m_p(3) * l3 * q3_hat * (-K_q * e_q3 - K_sOmega * e_sOmega3) - m_p(3) * q3_hat^2 * a_3;
% un_4 = m_p(4) * l4 * q4_hat * (-K_q * e_q4 - K_sOmega * e_sOmega4) - m_p(4) * q4_hat^2 * a_4;

un_i = [un_1,un_2,un_3,un_4];

% 绳锁转动速度导数
sOmega_dot = zeros(3,4);
% sOmega_dot_1 = -K_q * e_q1 - K_sOmega * e_sOmega1 - (q1' * sOmega1d) * q1_dot - q1_hat * q1_hat * omegad_dot;
sOmega_dot(:,1) = -K_q * e_q1 - K_sOmega * e_sOmega1 - (q1' * sOmega1d) * q1_dot;
sOmega_dot(:,2) = -K_q * e_q2 - K_sOmega * e_sOmega2 - (q2' * sOmega2d) * q2_dot;
sOmega_dot(:,3) = -K_q * e_q3 - K_sOmega * e_sOmega3 - (q3' * sOmega3d) * q3_dot;
sOmega_dot(:,4) = -K_q * e_q4 - K_sOmega * e_sOmega4 - (q4' * sOmega4d) * q4_dot;


% 求解u1_p
up_1 = miu1 + m_p(1) * l1 * norm(sOmega(:,1))^2 * q1 + m_p(1) * (q1 * q1') * a_1;
up_2 = miu2 + m_p(2) * l2 * norm(sOmega(:,2))^2 * q2 + m_p(2) * (q2 * q2') * a_2;
up_3 = miu3 + m_p(3) * l3 * norm(sOmega(:,3))^2 * q3 + m_p(3) * (q3 * q3') * a_3;
up_4 = miu4 + m_p(4) * l4 * norm(sOmega(:,4))^2 * q4 + m_p(4) * (q4 * q4') * a_4;


%% 计算无人机的力和力矩
% 进入到求解无人机的u和M
u1 = un_1 + up_1;
u2 = un_2 + up_2; 
u3 = un_3 + up_3;
u4 = un_4 + up_4;


% 姿态角度
x_att = zeros(3,4);
x_att(:,1) = [x_plant(1,1); x_plant(3,1); x_plant(5,1)]; 
x_att(:,2) = [x_plant(1,2); x_plant(3,2); x_plant(5,2)]; 
x_att(:,3) = [x_plant(1,3); x_plant(3,3); x_plant(5,3)]; 
x_att(:,4) = [x_plant(1,4); x_plant(3,4); x_plant(5,4)]; 

x_ome = zeros(3,4);
x_ome(:,1) = [x_plant(2,1); x_plant(4,1); x_plant(6,1)]; 
x_ome(:,2) = [x_plant(2,2); x_plant(4,2); x_plant(6,2)]; 
x_ome(:,3) = [x_plant(2,3); x_plant(4,3); x_plant(6,3)]; 
x_ome(:,4) = [x_plant(2,4); x_plant(4,4); x_plant(6,4)]; 


R_1 = eul2rotm_ZYX(x_att(:,1)); 
R_2 = eul2rotm_ZYX(x_att(:,2)); 
R_3 = eul2rotm_ZYX(x_att(:,3)); 
R_4 = eul2rotm_ZYX(x_att(:,4));

% 姿态旋转矩阵误差
Rd_3_1 = -u1/norm(u1);
Rd_3_2 = -u2/norm(u2);
Rd_3_3 = -u3/norm(u3); 
Rd_3_4 = -u4/norm(u4);

Rd_3_1_hat = skew(Rd_3_1); 
Rd_3_2_hat = skew(Rd_3_2);
Rd_3_3_hat = skew(Rd_3_3); 
Rd_3_4_hat = skew(Rd_3_4);

R_psid_1 = [1; 0; 0];
R_psid_2 = [1; 0; 0];
R_psid_3 = [1; 0; 0];
R_psid_4 = [1; 0; 0];

Rd_1_1 = -(Rd_3_1_hat * Rd_3_1_hat * R_psid_1) / norm(Rd_3_1_hat * Rd_3_1_hat * R_psid_1);
Rd_1_2 = -(Rd_3_2_hat * Rd_3_2_hat * R_psid_2) / norm(Rd_3_2_hat * Rd_3_2_hat * R_psid_2);
Rd_1_3 = -(Rd_3_3_hat * Rd_3_3_hat * R_psid_3) / norm(Rd_3_3_hat * Rd_3_3_hat * R_psid_3);
Rd_1_4 = -(Rd_3_4_hat * Rd_3_4_hat * R_psid_4) / norm(Rd_3_4_hat * Rd_3_4_hat * R_psid_4);


Rd_2_1 = (Rd_3_1_hat * R_psid_1) / norm(Rd_3_1_hat * R_psid_1);
Rd_2_2 = (Rd_3_2_hat * R_psid_2) / norm(Rd_3_2_hat * R_psid_2);
Rd_2_3 = (Rd_3_3_hat * R_psid_3) / norm(Rd_3_3_hat * R_psid_3);
Rd_2_4 = (Rd_3_4_hat * R_psid_4) / norm(Rd_3_4_hat * R_psid_4);


Rd_1 = [Rd_1_1, Rd_3_1, Rd_2_1];
Rd_2 = [Rd_1_2, Rd_3_2, Rd_2_2];
Rd_3 = [Rd_1_3, Rd_3_3, Rd_2_3];
Rd_4 = [Rd_1_4, Rd_3_4, Rd_2_4];
% % 预期角度
angled_p = zeros(3,4);
angled_p(:,1) = rotm2eul(Rd_1,'ZYX');
angled_p(:,2) = rotm2eul(Rd_2,'ZYX');
angled_p(:,3) = rotm2eul(Rd_3,'ZYX');
angled_p(:,4) = rotm2eul(Rd_4,'ZYX');
% angled_p = zeros(3,4);
% angled_p(:,1) = rotation_matrix_to_euler(Rd_1);
% angled_p(:,2) = rotation_matrix_to_euler(Rd_2);
% angled_p(:,3) = rotation_matrix_to_euler(Rd_3);
% angled_p(:,4) = rotation_matrix_to_euler(Rd_4);

% 预期角速度
angled_p_dot = zeros(3,4);
angled_p_dot(:,1) = (angled_p(:,1) - angleid_p(:,1)) / dt;
angled_p_dot(:,2) = (angled_p(:,2) - angleid_p(:,2)) / dt;
angled_p_dot(:,3) = (angled_p(:,3) - angleid_p(:,3)) / dt;
angled_p_dot(:,4) = (angled_p(:,4) - angleid_p(:,4)) / dt;
%% 调参
k_R = zeros(3,3,4);
k_R(:,:,1) = diag([19.8, 28, 20.5]);
k_R(:,:,2) = diag([15, 24.8, 23.8]);
k_R(:,:,3) = diag([16.5, 20, 20.0]);
k_R(:,:,4) = diag([19.8, 28, 20.5]);

k_omega = zeros(3,3,4);
k_omega(:,:,1) = diag([0.8, 0.6, 1.2]);
k_omega(:,:,2) = diag([1.0, 0.5, 1.0]);
k_omega(:,:,3) = diag([1.0, 0.6, 1.0]);
k_omega(:,:,4) = diag([0.8, 0.6, 1.2]);

% 无人机控制力矩
M_1 = attitude_control(x_att(:,1), angled_p(:,1), angled_p_dot(:,1), x_ome(:,1), k_omega(:,:,1), k_R(:,:,1), a_p, b_p);
M_2 = attitude_control(x_att(:,2), angled_p(:,2), angled_p_dot(:,2), x_ome(:,2), k_omega(:,:,2), k_R(:,:,2), a_p, b_p);
M_3 = attitude_control(x_att(:,3), angled_p(:,3), angled_p_dot(:,3), x_ome(:,3), k_omega(:,:,3), k_R(:,:,3), a_p, b_p);
M_4 = attitude_control(x_att(:,4), angled_p(:,4), angled_p_dot(:,4), x_ome(:,4), k_omega(:,:,4), k_R(:,:,4), a_p, b_p);

f_1 = u1' * R_1 * e3;
f_2 = u2' * R_2 * e3;
f_3 = u3' * R_3 * e3;
f_4 = u4' * R_4 * e3;

F_plant = -[f_1, f_2, f_3, f_4];
M_plant = [M_1, M_2, M_3, M_4];


% 计算完成之后，更新绳索sOmega
sOmega(:,1) = sOmega(:,1) + dt * sOmega_dot(:,1);
sOmega(:,2) = sOmega(:,2) + dt * sOmega_dot(:,2);
sOmega(:,3) = sOmega(:,3) + dt * sOmega_dot(:,3);
sOmega(:,4) = sOmega(:,4) + dt * sOmega_dot(:,4);

%% 用于保存上一次的结果用来求导
% 更新下一次的q_i
qi = [q1, q2, q3, q4];
% qi_d
qi_d = [q1d, q2d, q3d, q4d];

% 更新sOmega_d
sOmega_d(:,1) = sOmega1d;
sOmega_d(:,2) = sOmega2d;
sOmega_d(:,3) = sOmega3d;
sOmega_d(:,4) = sOmega4d;

% 更新omegaid
angleid_p(:,1) = angled_p(:,1);
angleid_p(:,2) = angled_p(:,2);
angleid_p(:,3) = angled_p(:,3);
angleid_p(:,4) = angled_p(:,4);

Ome_L = x_Ome_L;

end

%% ================== 斜对称矩阵生成函数 ==================
function S = skew(v)
    % v 为 3x1 向量, S 则为 3x3 反对称矩阵,满足 S*w = v x w
    S = [   0,  -v(3),  v(2);
         v(3),     0,   -v(1);
        -v(2),   v(1),    0   ];
end

function v = vee(S)
    % S 是 3x3 的斜对称矩阵
    % 返回 v, 满足 S = skew(v)
    % 即 S = v^wedge, 而 v = S^vee
    v = [S(3,2); 
         S(1,3);
         S(2,1)];
end

function R = eul2rotm_ZYX(angles)
    % angles = [roll; pitch; yaw]，单位：弧度
    % 输出 R：3x3旋转矩阵
    %
    % 如果你的 angles 顺序或含义不同，
    % 请自行修改这部分的映射关系。

    roll  = angles(1);
    pitch = angles(2);
    yaw   = angles(3);

    % 基本旋转矩阵
    Rz = [ cos(yaw), -sin(yaw), 0;
           sin(yaw),  cos(yaw), 0;
           0,         0,        1 ];

    Ry = [ cos(pitch),  0, sin(pitch);
           0,           1, 0;
          -sin(pitch),  0, cos(pitch) ];
    
    Rx = [ 1, 0,         0;
           0, cos(roll), -sin(roll);
           0, sin(roll),  cos(roll) ];

    % 按 ZYX 顺序做右乘
    % R = Rz(yaw)*Ry(pitch)*Rx(roll)
    R = Rz * Ry * Rx;
end

function euler_angles = rotation_matrix_to_euler(R)
    % 确保矩阵是3x3
    if size(R,1) ~= 3 || size(R,2) ~= 3
        error('输入的旋转矩阵必须是3x3矩阵');
    end
    
    % 提取旋转矩阵的元素
    R11 = R(1,1);
    R12 = R(1,2);
    R13 = R(1,3);
    R21 = R(2,1);
    R22 = R(2,2);
    R23 = R(2,3);
    R31 = R(3,1);
    R32 = R(3,2);
    R33 = R(3,3);
    
    % 计算欧拉角（ZYX顺序）
    yaw = atan2(R21, R11);  % 绕Z轴的旋转
    pitch = atan2(-R31, sqrt(R11^2 + R21^2));  % 绕Y轴的旋转
    roll = atan2(R32, R33);  % 绕X轴的旋转
    
    % 返回欧拉角
    euler_angles = [roll, pitch, yaw]';  % 顺序为 [roll, pitch, yaw]
end

function M = attitude_control(x_att, angled_p, angled_p_dot, x_ome, k_omega, k_R, a_p, b_p)
    e1_Att = wrapToPi(x_att - angled_p);
    alpha = (angled_p_dot - k_omega * e1_Att);
    e1_Att_dot = x_ome - angled_p_dot;
    e2_Att = x_ome - alpha;
    
    fx_phi = a_p(1) * x_ome(2) * x_ome(3);
    gx_phi = b_p(1);
    
    fx_theta = a_p(2) * x_ome(1) * x_ome(3);
    gx_theta = b_p(2);

    fx_psi = a_p(3) * x_ome(1) * x_ome(2);
    gx_psi = b_p(3);
    
    fx = [fx_phi fx_theta fx_psi]';
    gx = [gx_phi, gx_theta, gx_psi]';

    M = zeros(3,1);
    M(1) = (-e1_Att(1) - fx(1) + e1_Att_dot(1) - k_R(1,1) * e2_Att(1)) / gx(1);
    M(2) = (-e1_Att(2) - fx(2) + e1_Att_dot(2) - k_R(2,2) * e2_Att(2)) / gx(2);
    M(3) = (-e1_Att(3) - fx(3) + e1_Att_dot(3) - k_R(3,3) * e2_Att(3)) / gx(3);
    
end
